#include <reg52.h>
#include <intrins.h>
#include "INA219_Driver.h"
#include "SegmentDisplayLite.h"
#define FILTER_N 5

float current = 0;
float lastAvg = 0;
char cnt = 0;
float CurrentValues[] = {0, 0, 0, 0};

float FloatAbs(float m){
	if(m < 0)
		return -m;
	return m;
}
float filter(float res){
	short i;
	float avg = 0;
    float sum = 0;
	
	
    CurrentValues[cnt] = res;
    for(i = 0; i < 5; i++)
        sum += CurrentValues[i];
    avg = sum / 4;
    if(cnt == 3)
        cnt = 0;
    else
        cnt++;
	
	if(FloatAbs(avg - lastAvg) > 0.005){
		lastAvg = avg;
	}
	
    return lastAvg;
	
}
void main(){
	float res;
	
	INA219_Init();
	while(1){
		res = INA219_GetCurrent();
		res /= 10000;
		current = filter(res);
		
		NumDisplay(current);
		IndexedDisplay(10,3,0);
	}
		
	
}